#include "SENSOR.h"


//public

SENSOR::SENSOR(int pinin, int pinecho, int pintrig) {
  pinIn = pinin;
  pinEcho = pinecho;
  pinTrig = pintrig;
}


void SENSOR::init() {
  pinMode(pinIn, INPUT); //红外线引脚初始化

  //激光传感器
  Wire.begin();//加入IIC总线，设置为主机

  //超声波传感器
  pinMode(pinEcho, INPUT);
  pinMode(pinTrig, OUTPUT);
}



//读取红外线传感器测量值
int SENSOR::InfraredRead() {
  uint16_t value = 0;

  for (int i = 0; i < 5; i++) { //平均滤波
    value = value + analogRead(pinIn);
    delayMicroseconds(2000);
  }

  return value / 5;
}



//读取激光传感器测量值
int SENSOR::LaserRead() {
  for (uint8_t i = 0; i < 5; i++) {
    IICRead(REAL_TIME_DISTANCE, RX_BUF, 2);
    high_byte += RX_BUF[0];
    low_byte += RX_BUF[1];
  }
  high_byte /= 5;
  low_byte /= 5;
  distance = high_byte;
  distance <<= 8;
  distance += low_byte;
  high_byte = 0;
  low_byte = 0;
  return distance;
}


//读取超声波传感器
int SENSOR::UltrasonicRead() {
  for (int i = 0; i < 5; i++) {
    // 通过TrigPin发送脉冲，触发US_100测距
    digitalWrite(pinTrig, HIGH);
    delayMicroseconds(50);//设置脉冲宽度为50us(>10us)
    digitalWrite(pinTrig, LOW); //结束脉冲

    lenSum = lenSum + pulseIn(pinEcho, HIGH); //计算US-100返回的脉冲宽度

  }

  Time_Echo_us = lenSum / 5;
  lenSum = 0;

  if ((Time_Echo_us < 60000) && (Time_Echo_us > 1)) //有效脉冲宽度（1,60000）
  {
    Len_mm = (Time_Echo_us * 34 / 100) / 2;

  }


  return Len_mm;
}



//private

//往IIC从机写入数据
//addr(参数寄存器地址),datbuf（数据缓冲区地址),cnt(数据长度)
String SENSOR::IICWrite(uint8_t addr, uint8_t*datbuf, uint16_t cnt) {
  uint8_t result = 0 ;
  Wire.beginTransmission(82);//指定传输从机地址，TOF1默认地址为#82
  result = Wire.write(addr);
  if (result == 0) return "write error!";
  delay(1);//数据手册建议t>=32us
  result = Wire.write(datbuf, cnt);
  if (result == 0) return "write error!";
  Wire.endTransmission();
}

//读取IIC从机数据
void SENSOR::IICRead(uint8_t addr, uint8_t* datbuf, uint16_t cnt) {
  Wire.beginTransmission(82);
  Wire.write(addr);
  Wire.endTransmission();
  delay(1);
  Wire.requestFrom(82, cnt); //向指定从机请求数据
  for (uint8_t i = 0; i < cnt; i++) {
    *datbuf = Wire.read();
    datbuf++;
  }
}
